import numpy as np
import math

# 距离测量参数
deltaX = 0.325
deltaY = 0.226
deltaZ = 0.

# 相机内参矩阵
innerMatrx = np.array([[963.45700909,0.,632.41394014],[ 0.,931.24907869,361.0086165 ],[ 0.,0.,1. ]])

# 绘图颜色
colorlist = [(255,0,0),(0,255,0),(0,0,255)]

# 雷达坐标转换为相机坐标
def Radar2Camera(RadarTag):

    distance = RadarTag['distance'] * 1.0 / 100
    Radians = math.radians(RadarTag['angel'])

    Xc = deltaX + distance*math.sin(Radians)
    Yc = deltaY
    Zc = distance*math.cos(Radians)

    caremaPos = np.array([Xc,Yc,Zc])

    pixPos = np.matmul(innerMatrx,caremaPos) / caremaPos[2]

    RadarTag['X'] = pixPos[0]
    RadarTag['Y'] = pixPos[1]
    return RadarTag